/*
 * FreeRTOS V202107.00
 * Copyright (C) 2020 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 * http://www.FreeRTOS.org
 * http://aws.amazon.com/freertos
 *
 * 1 tab == 4 spaces!
 */

/******************************************************************************
 * This project provides two demo applications.  A simple blinky style project,
 * and a more comprehensive test and demo application.  The
 * mainCREATE_SIMPLE_BLINKY_DEMO_ONLY setting (defined in this file) is used to
 * select between the two.  The simply blinky demo is implemented and described
 * in main_blinky.c.  The more comprehensive test and demo application is
 * implemented and described in main_full.c.
 *
 * This file implements the code that is not demo specific, including the
 * hardware setup and standard FreeRTOS hook functions.
 *
 * ENSURE TO READ THE DOCUMENTATION PAGE FOR THIS PORT AND DEMO APPLICATION ON
 * THE http://www.FreeRTOS.org WEB SITE FOR FULL INFORMATION ON USING THIS DEMO
 * APPLICATION, AND ITS ASSOCIATE FreeRTOS ARCHITECTURE PORT!
 *
 */

/* Scheduler include files. */
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"

/* Standard demo includes. */
#include "TimerDemo.h"
#include "QueueOverwrite.h"
#include "EventGroupsDemo.h"
#include "IntSemTest.h"
#include "TaskNotify.h"

#include "main.h"

/* Library includes. */
#include <stdio.h>
#include "hardware/gpio.h"
#include "hardware/clocks.h"
#include "ws2812.pio.h"
#include "pico/stdlib.h"
#if ( mainRUN_ON_CORE == 1 )
#include "pico/multicore.h"
#endif

/*-----------------------------------------------------------*/
/* Priorities at which the tasks are created. */
#define mainTEST_TASK_PRIORITY		        ( tskIDLE_PRIORITY + 1 )
#define mainTEST_RGB_PRIORITY		        ( tskIDLE_PRIORITY + 1 )
#define mainUART_CLI_TASK_PRIORITY          ( tskIDLE_PRIORITY + 2 )
/* The LED toggled by the Rx task. */
#define mainTASK_LED						( PICO_DEFAULT_LED_PIN )

/*
 * Configure the hardware as necessary to run this demo.
 */
static void prvSetupHardware( void );

/*
 * Register commands that can be used with FreeRTOS+CLI through the UDP socket.
 * The commands are defined in CLI-commands.c.
 */
extern void vRegisterCLICommands( void );

/*
 * The tasks as described in the comments at the top of this file.
 */
static void prvTestTask( void *pvParameters );
static void prvRGBTask( void *pvParameters );
void vUARTCommandConsoleStart( uint16_t usStackSize,
                               UBaseType_t uxPriority );

/* Prototypes for the standard FreeRTOS callback/hook functions implemented
within this file. */
void vApplicationMallocFailedHook( void );
void vApplicationIdleHook( void );
void vApplicationStackOverflowHook( TaskHandle_t pxTask, char *pcTaskName );
void vApplicationTickHook( void );

TaskHandle_t TestTaskHandle;

/*-----------------------------------------------------------*/
int main( void )
{
    /* Configure the hardware ready to run the demo. */
    prvSetupHardware();

    UBaseType_t uxCoreAffinityMask;

    printf(" Starting smp cli.\n");

    xTaskCreate( prvRGBTask, "RGB", configMINIMAL_STACK_SIZE, NULL, mainTEST_RGB_PRIORITY, NULL);

    /* Start the two tasks as described in the comments at the top of this
    file. */
    xTaskCreate( prvTestTask,				        /* The function that implements the task. */
                "TEST", 							/* The text name assigned to the task - for debug only as it is not used by the kernel. */
                configMINIMAL_STACK_SIZE, 			/* The size of the stack to allocate to the task. */
                NULL, 								/* The parameter passed to the task - not used in this case. */
                mainTEST_TASK_PRIORITY, 	        /* The priority assigned to the task. */
                &TestTaskHandle );			        /* The task handle is not required, so NULL is passed. */

    // /* Define the core affinity mask such that this task can only run on core 0 and core 1 */
    // uxCoreAffinityMask = ( ( 1 << 0 ) );

    // /* Set the core affinity mask for the task. */
    // vTaskCoreAffinitySet( TestTaskHandle, uxCoreAffinityMask );

    // vUARTCommandConsoleStart(configMINIMAL_STACK_SIZE, mainUART_CLI_TASK_PRIORITY);

    // /* Register commands with the FreeRTOS+CLI command interpreter. */
    // vRegisterCLICommands();

    /* Start the tasks and timer running. */
    vTaskStartScheduler();

	/* If all is well, the scheduler will now be running, and the following
	line will never be reached.  If the following line does execute, then
	there was insufficient FreeRTOS heap memory available for the Idle and/or
	timer tasks to be created.  See the memory management section on the
	FreeRTOS web site for more details on the FreeRTOS heap
	http://www.freertos.org/a00111.html. */
	for( ;; );
}
/*-----------------------------------------------------------*/

static void prvSetupHardware( void )
{
    stdio_init_all();
    gpio_init(mainTASK_LED);
    gpio_set_dir(mainTASK_LED, 1);
    gpio_put(mainTASK_LED, !PICO_DEFAULT_LED_PIN_INVERTED);
}

void put_pixel(uint32_t pixel_grb)
{
    pio_sm_put_blocking(pio0, 0, pixel_grb << 8u);
}

void put_rgb(uint8_t red, uint8_t green, uint8_t blue)
{
    uint32_t mask = (green << 16) | (red << 8) | (blue << 0);
    put_pixel(mask);
}

static void prvTestTask( void *pvParameters )
{
    TickType_t xNextWakeTime;
    UBaseType_t uxNetworkingCoreAffinityMask;

	/* Remove compiler warning about unused parameter. */
	( void ) pvParameters;

	/* Initialise xNextWakeTime - this only needs to be done once. */
	xNextWakeTime = xTaskGetTickCount();

	for( ;; )
	{
        printf("Core %d - Thread '%s'\n", get_core_num(), pcTaskGetName(xTaskGetCurrentTaskHandle()));
        // /* Get the core affinity mask for the networking task. */
        uxNetworkingCoreAffinityMask = vTaskCoreAffinityGet( xTaskGetCurrentTaskHandle() );

        /* Here is a hypothetical scenario, just for the example. Assume that we
         * have 2 cores - Core 0 and core 1. We want to pin the application task to
         * the core that is not the networking task core to ensure that the
         * application task does not interfere with networking. */
        if( ( uxNetworkingCoreAffinityMask & ( 1 << 0 ) ) != 0 )
        {
            /* The networking task can run on core 0, pin our task to core 1. */
            vTaskCoreAffinitySet( xTaskGetCurrentTaskHandle(), ( 1 << 1 ) );
        }
        else
        {
            /* Otherwise, pin our task to core 0. */
            vTaskCoreAffinitySet( xTaskGetCurrentTaskHandle(), ( 1 << 0 ) );
        }
        xTaskDelayUntil(&xNextWakeTime, pdMS_TO_TICKS(1000));
	}
}
/*-----------------------------------------------------------*/

static void prvRGBTask( void *pvParameters )
{
	PIO pio = pio0;
	int sm = 0;
	uint8_t offset = pio_add_program(pio, &ws2812_program);
	uint8_t cnt = 0;

	/* Remove compiler warning about unused parameter. */
	( void ) pvParameters;

	printf("RP2040-Zero WS2812 Test.\n");
	ws2812_program_init(pio, sm, offset, 16, 800000, true);

	for( ;; )
	{
        for (cnt = 0; cnt < 0xff; cnt++)
        {
            put_rgb(cnt, 0xff - cnt, 0);
            vTaskDelay(pdMS_TO_TICKS(3));
        }
        for (cnt = 0; cnt < 0xff; cnt++)
        {
            put_rgb(0xff - cnt, 0, cnt);
            vTaskDelay(pdMS_TO_TICKS(3));
        }
        for (cnt = 0; cnt < 0xff; cnt++)
        {
            put_rgb(0, cnt, 0xff - cnt);
            vTaskDelay(pdMS_TO_TICKS(3));
        }
        printf("Core %d - Thread '%s'\n", get_core_num(), pcTaskGetName(xTaskGetCurrentTaskHandle()));
	}
}
/*-----------------------------------------------------------*/

void vApplicationMallocFailedHook( void )
{
    /* Called if a call to pvPortMalloc() fails because there is insufficient
    free memory available in the FreeRTOS heap.  pvPortMalloc() is called
    internally by FreeRTOS API functions that create tasks, queues, software
    timers, and semaphores.  The size of the FreeRTOS heap is set by the
    configTOTAL_HEAP_SIZE configuration constant in FreeRTOSConfig.h. */

    /* Force an assert. */
    configASSERT( ( volatile void * ) NULL );
}
/*-----------------------------------------------------------*/

void vApplicationStackOverflowHook( TaskHandle_t pxTask, char *pcTaskName )
{
    ( void ) pcTaskName;
    ( void ) pxTask;

    /* Run time stack overflow checking is performed if
    configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2.  This hook
    function is called if a stack overflow is detected. */

    /* Force an assert. */
    configASSERT( ( volatile void * ) NULL );
}
/*-----------------------------------------------------------*/

void vApplicationIdleHook( void )
{
    volatile size_t xFreeHeapSpace;

    /* This is just a trivial example of an idle hook.  It is called on each
    cycle of the idle task.  It must *NOT* attempt to block.  In this case the
    idle task just queries the amount of FreeRTOS heap that remains.  See the
    memory management section on the http://www.FreeRTOS.org web site for memory
    management options.  If there is a lot of heap memory free then the
    configTOTAL_HEAP_SIZE value in FreeRTOSConfig.h can be reduced to free up
    RAM. */
    xFreeHeapSpace = xPortGetFreeHeapSize();

    /* Remove compiler warning about xFreeHeapSpace being set but never used. */
    ( void ) xFreeHeapSpace;
}
/*-----------------------------------------------------------*/

void vApplicationTickHook( void )
{
#if mainCREATE_SIMPLE_BLINKY_DEMO_ONLY == 0
    {
        /* The full demo includes a software timer demo/test that requires
        prodding periodically from the tick interrupt. */
        #if (mainENABLE_TIMER_DEMO == 1)
        vTimerPeriodicISRTests();
        #endif

        /* Call the periodic queue overwrite from ISR demo. */
        #if (mainENABLE_QUEUE_OVERWRITE == 1)
        vQueueOverwritePeriodicISRDemo();
        #endif

        /* Call the periodic event group from ISR demo. */
        #if (mainENABLE_EVENT_GROUP == 1)
        vPeriodicEventGroupsProcessing();
        #endif

        /* Call the code that uses a mutex from an ISR. */
        #if (mainENABLE_INTERRUPT_SEMAPHORE == 1)
        vInterruptSemaphorePeriodicTest();
        #endif

        /* Call the code that 'gives' a task notification from an ISR. */
        #if (mainENABLE_TASK_NOTIFY == 1)
        xNotifyTaskFromISR();
        #endif
    }
#endif
}
/*-----------------------------------------------------------*/